
#include "nav_controller/sensor/laser_util.hpp"

namespace nav_controller
{

    LaserUtil::LaserUtil(rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
    {
        logger_ = node->get_logger();
        laser_sub_ = node->create_subscription<sensor_msgs::msg::LaserScan>(
            "scan", rclcpp::SensorDataQoS(),
            std::bind(&LaserUtil::laser_callback, this, std::placeholders::_1));
    }
    LaserUtil::~LaserUtil()
    {
        laser_sub_.reset();
        laser_msg_.reset();
    }

    /**
     * 接收雷达数据
     */
    void LaserUtil::laser_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        laser_msg_ = msg;
    }

    float LaserUtil::get_mid_distance()
    {
        // 假设激光雷达的角度范围是 -180 度到 180 度，正前方是 0 度
        int index = (laser_msg_->angle_min == 0)? 0 : (laser_msg_->angle_min + laser_msg_->angle_increment);
        // 获取正前方的距离
        float distance = laser_msg_->ranges[index];
        RCLCPP_INFO(logger_, "laser_msg_: %f", distance);
        return distance;
    }

}